#-*- coding: UTF-8 -*- 
#!/usr/bin/python
import rospy
# 导航
from ros.navigation import Navigation 
# 简单位移
from ros.Control import Control
# from util.utils_cv import *
# from util.utils_process import *
# 多线程
import threading
from threading import Thread
# 拍摄图片
from cv_2022 import *
# 文字识别
from ocr.detect_test import *
# 雷达测距
# from ros.get_laser2 import adopt_forward
from single_laser import *
# 日志记录模块
from log.pylog import log
LOGS_INFO = log.info
# 机械臂抓取动作
from dobot_arm import *


sentinel = threading.Event()
sentinel.set()
rospy.init_node("process", anonymous=True,disable_signals=True)
navigation = Navigation()
control = Control()
in_start = True
(x, y, z, r, j1, j2, j3, j4) = device.pose()
rotate_to_inside = Thread(target=device.rotate_to,args=[-90, 0.0, 0.0, 0.0, True]) # 机械臂转向内测
rotate_to_outside = Thread(target=device.rotate_to,args=[90, 0.0, 0.0, 0.0, True]) # 机械臂转向外侧，实际使用需重复定义
drob_thread = Thread(target=drob_test,args=[])
grob_thread = Thread(target=grob_route,args=[])
navigation_thread = Thread(target=navigation.send_goal,args=[])
adopt_forward_thread = Thread(target=adopt_forward,args=[])
drob_route3_thread = Thread(target=drob_route3,args=[])
drob_route3_thread = Thread(target=drob_route3,args=[])
control_forward = Thread(target=control.forward,args=[] )
subscribe_main_thread = Thread(target=subscribe_main,args=[])
subscribe_main_thread.start()

# 初始化投递盒-序号以左上角为0,顺时针为正向一直到左下角的9号箱,起初十个均未知
# drop_boxes = ["unknown"] * 8
drop_boxes = ["湖南","广东","浙江","江苏","福建","河南","安徽","四川"]

# middle_left = 0.45
# middle_right = 2.850
# middle_y = 1.16
# cur_pos = middle_left

platform_center = [1.573,0.9,90]

# dot_positionoints = [[0.480,1.010], [0.710,1.010],[0.945,1.010],[1.199,1.010],[1.465,1.010],   # 0 1 2 3 4 
#                [1.700,1.010],[2.015, 1.010],[2.262,1.010],[2.560, 1.010], [2.785,1.010]]  # 5 6 7 8 9
# dot_positionoints = [[0.460,1.00], [0.720,0.980],[0.97,0.980],[1.220,0.980],[1.440,0.980],   # 0 1 2 3 4 
#             [1.700,0.980],[1.955, 0.980],[2.290,0.980],[2.575, 0.990], [2.805,0.990]]
# dot_positionoints = [[0.490,0.990], [0.710,0.970],[1.000,0.970],[1.270,0.970],[1.535,0.970],   # 0 1 2 3 4 
#                [1.750,0.970],[2.045, 0.970],[2.350,0.970],[2.620, 0.970], [2.825,0.970]]  # 5 6 7 8 9  
# dot_positionoints = [[0.490,0.990], [0.710,0.980],[1.000,0.980],[1.270,0.980],[1.500,0.980],   # 0 1 2 3 4 
#             [1.750,0.980],[2.025, 0.980],[2.340,0.980],[2.620, 0.980], [2.875,0.980]]  

#  目前整体位置需要再向左侧移动0.1~0.2
dot_positionoints = [[0.480,0.970], [0.720,0.970],[0.99,0.970],[1.265,0.970],[1.510,0.970],   # 0 1 2 3 4 
               [1.800,0.970],[2.080, 0.970],[2.305,0.970],[2.620, 0.970], [2.865,0.970]]  # 5 6 7 8 9
LOGS_INFO("robot init finish!")


# 相机预热
# pre_take_pic()
furthest_pos = 0
start_time  = time.time()
for dot_position ,[cur_pos,middle_y] in enumerate(dot_positionoints):
    # 由于未检测到快递盒会发生Slide滑行,可能会提前抓取到最后几个快递盒子，导致最后一个抓取点无东西可抓，故需要检测距离及时终止
    if furthest_pos >= dot_positionoints[-1][0] - 0.01:
        break
    furthest_pos = cur_pos
    # 车身存储快递盒序号
    store_box_num = 0
    # 1. 小车从出发区出发，进入分捡台最左端
    # dot_position = "g"+str(i)
    LOGS_INFO("robot arm rotate_to -90 0 0 0")
    rotate_to_outside = Thread(target=device.rotate_to,args=[-90, 0.0, 0.0, 0.0, True]) # 机械臂转向分拣台
    rotate_to_outside.start()
    LOGS_INFO("robot MOVE to g{}".format(dot_position))
    navigation.send_position([cur_pos,middle_y],-90)
    if control_forward.isAlive(): #线程存活
        control_forward.join() # 结束线程
    # rospy.sleep(0.5)
    adopt_forward(-0.3,0.125)
    # adopt_forward(0.20)
    # 雷达测距，贴近目标
    # from_wall_dis = obstacle.get_scan()
    # LOGS_INFO("check grab distance from plat: {}".format(from_wall_dis))
    # if(from_wall_dis > 0.30):
    #     adopt_forward(0.10,0.30)
    # time.sleep(0.5)
    if rotate_to_outside.isAlive(): #线程存活
        rotate_to_outside.join() # 结束线程

    
    all_detect_word =[]
    LOGS_INFO("robot arm toward platform")
    while( store_box_num < 2):
        #   机械臂转向外侧，面向分捡台   20 , 10
        device.rotate_to(-90,0,5,0, True)
        # 2. 开启摄像头左右识别
        rospy.sleep(0.3)
        #   相机照相
        box_color_img = pre_take_pic()

        # 3. 轮廓检测, 为文字识别作准备
        # cnt_centre_list: [ [x,y], area_size ]
        # detect_word_list: [安徽 ,……]
        # TODO：精度和耗时有待优化，精度：尝试其他模型； 耗时：采用自适应旋转，可减少一次识别
        # TODO: 模型识别时 可以进行多线程抓取 
        img,cnt_centre_list,boxes_list,extra_big_box = detect_prepare(box_color_img,1)
        LOGS_INFO("cnt_centre_list:{}".format(cnt_centre_list))
        LOGS_INFO("extra_big_box:{}".format(extra_big_box))

        # TODO: 保存标记后的图片 
        # mark_img = img.copy()
        # mark_img = 

        # 4.机械臂抓取
        if len(cnt_centre_list) > 2:
            cnt_centre_list = cnt_centre_list[:2]
        grob_thread = Thread(target=grob_route_new,args=[cnt_centre_list,store_box_num,1])
        grob_thread.start()
        detect_boxes_list = []
        for i,centre in enumerate(cnt_centre_list):
            if store_box_num > 1:
                break
            store_box_num += 1
            LOGS_INFO("store_box_num: {}".format(store_box_num))
        # 5. 文字识别
        detect_word_list = Final_Match_Detect(img,extra_big_box,"g"+str(dot_position))
        LOGS_INFO("detected: {}".format(detect_word_list))
        word_len = 0
        while(len(all_detect_word) < 2  and word_len < len(detect_word_list)):
            all_detect_word.append(detect_word_list[word_len])
            word_len += 1
        if grob_thread.isAlive(): #线程存活
            grob_thread.join() # 结束线程

        # 当抓取数量未满2个时，机械臂左右查看，仍未满足，最后侧向移动
        if store_box_num < 2 : 
            extra_grab_position = [[-43.17, 16.38, 6.2,0],[-123.24, 4.40, -1.45,0]]
            for angle,[x,y,z,r] in enumerate(extra_grab_position):
                if store_box_num >=2 :
                    break

                device.rotate_to(x,y,z,r,True)
                rospy.sleep(0.3)
                #   相机照相
                box_color_img = pre_take_pic()
                img,cnt_centre_list,boxes_list,extra_big_box = detect_prepare(box_color_img,1)
                LOGS_INFO("cnt_centre_list:{}".format(cnt_centre_list))
                LOGS_INFO("extra_big_box:{}".format(extra_big_box))
                if len(cnt_centre_list) > 1:
                    cnt_centre_list = cnt_centre_list[:1]
                grob_thread = Thread(target=grob_route_new,args=[cnt_centre_list,store_box_num,angle])
                grob_thread.start()
                store_box_num += 1
                LOGS_INFO("store_box_num: {}".format(store_box_num))
                detect_word_list = Final_Match_Detect(img,extra_big_box,"Extra_g"+str(dot_position))
                if detect_word_list != []:
                    LOGS_INFO("detected: {}".format(detect_word_list))
                    all_detect_word.append(detect_word_list[0])
                if grob_thread.isAlive(): #线程存活
                    grob_thread.join() # 结束线程
            if store_box_num < 2 :
                control.forward(0.6)
                navigation.send_position([furthest_pos,middle_y],-90)
                rospy.sleep(0.5)
                adopt_forward(-0.3,0.125)
                furthest_pos += 0.3
            # if furthest_pos >= dot_positionoints[-1][0] - 0.01:
            #     break
    control_forward = Thread(target=control.forward,args=[0.5] )
    control_forward.start()
    # control.forward(0.5)             
    # 5.当车载有两个快递盒时，进行投放
    #   反转识别列表，按顺序投递
    # all_detect_word.reverse()
    # layer 左0 右1
    cnt_centre_list = []
    last_region = None
    for layer, region in enumerate(all_detect_word):
        try:
            # 上次的投递地区
            # 保留原地 标志
            stay_flag = False
            if layer == 0:
                last_region = region
            if layer == 1 and last_region == region:  
                stay_flag = True

            region_index = drop_boxes.index(region)
            # control.forward(0.4)
            rotate_to_inside = Thread(target=device.rotate_to,args=[90,0,5,0, True]) # 机械臂转向内测
            rotate_to_inside.start()
            if not stay_flag:
                # 必须到达投递盒子 才能抓取快递盒
                navigation_thread = Thread(target=navigation.send_goal,args=["p" + str(region_index)])
                navigation_thread.start()
                # navigation.send_goal("p" + str(region_index))
                LOGS_INFO("send box to {}".format(region))
                # if rotate_to_inside.isAlive(): #线程存活
                #     rotate_to_inside.join() # 结束线程
                if control_forward.isAlive(): #线程存活
                    control_forward.join() # 结束线程
                if navigation_thread.isAlive(): #线程存活
                    navigation_thread.join() # 结束线程
                # 自适应靠近距离
                adopt_forward_thread = Thread(target=adopt_forward,args=[-0.3,0.11])
                adopt_forward_thread.start()
                # adopt_forward(-0.3,0.11)
            # else:
            #     if rotate_to_inside.isAlive(): #线程存活
            #         rotate_to_inside.join() # 结束线程
            # 一次性保存两个快递盒的坐标，去除第二次识别，与deliver不冲突
            # if layer == 0:
            #     deliver_img = pre_take_pic()
            #     # print(deliver_img.shape)
            #     if deliver_img is not None:
            #         deliver_img[0:300, 0:pipeline_w] = 0
            #         cnt_centre_list= detect_prepare(deliver_img,1)[1]
            #         cnt_centre_list.reverse()

            #     LOGS_INFO("deliver detect cnt: {}".format(cnt_centre_list))
            # LOGS_INFO("layer:{}".format(layer))
            # 投递快递
            drob_finish_flag = False
            drob_route3(layer)
            drob_finish_flag = True
            if adopt_forward_thread.isAlive(): #线程存活
                    adopt_forward_thread.join() # 结束线程
            while not drob_finish_flag:
                drob_route3_thread = Thread(target=drob_route3,args=[layer,1])
                drob_route3_thread.start()
            
            # drob_route3(layer,1)
            # 如果是第一层投递，并且检测到的地区相同，则跳过前进
            if layer == 0 and len(all_detect_word) > 1:
                if all_detect_word[0] == all_detect_word[1]:
                    continue
            #投递完成 前进0.4m
            rospy.sleep(0.5)
            control_forward = Thread(target=control.forward,args=[0.6] )
            control_forward.start()
            # control.forward(0.6)
            if drob_route3_thread.isAlive(): #线程存活
                    drob_route3_thread.join() # 结束线程
            # drob_test(layer)
            # 检测是否投递成功，有一定耗时，可取消
            # deliver_check(layer)
            
            # 提前抓取
            # drob_thread = Thread(target=drob_test,args=[layer])
            # drob_thread.start()

            # 小距离贴近矫正
            # from_wall_dis = obstacle.get_scan()
            # rospy.loginfo("laser_dis: %f",from_wall_dis)
            # if(from_wall_dis > 0.30):
            #     adopt_forward(0.10,0.30)    
            # if drob_thread.isAlive(): #线程存活
            #     drob_thread.join() # 结束线程
            # 最终投放快递盒
            # device.suck(False)


        except Exception as e:
            traceback.print_exc()
            print(e.__traceback__.tb_frame.f_globals["__file__"],e.__traceback__.tb_lineno)   # 发生异常所在的文件 ,异常所在的行数
    
    # 移动下一个抓取点
    # control.forward(0.4)
    
    # TODO： 完成后删除 break
    # break
    # 6. 单次投放完，返回下一个任务点继续识别抓取 


# Final 任务完成，返回出发区
control_forward = Thread(target=control.forward,args=[0.5] )
control_forward.start()
rotate_to_inside = Thread(target=device.rotate_to,args=[90,0,30,0, True]) # 机械臂转向内测
rotate_to_inside.start()
navigation.send_goal("start")
if rotate_to_inside.isAlive(): #线程存活
    rotate_to_inside.join() # 结束线程
if control_forward.isAlive(): #线程存活
    control_forward.join() # 结束线程
end_time = time.time()
LOGS_INFO("robot tasks FINISH: cost time {}".format((end_time - start_time) / 60) )